#include "modules/CtrlModule/ActivePlanner/RoomDividerPlanner.h"
#include "modules/CtrlModule/ActivePlanner/ZigzagPlanner.h"
#include "lib/Time.h"

namespace behavior_controller
{
RoomDividerPlanner room_divider_planner;

void RoomDividerPlanner::run(double &linear_vel, double &angular_vel, bool &finished)
{
    linear_vel =
    angular_vel = 0;
    if (!online_flag)
    {
        std::shared_ptr<nav2_msgs::srv::GetMapNav2_Response> srv_goal = nullptr;
        if (!ipa_room_plan->dataDone())
        {
            std::vector<double> robot_pos;
            robot_pos.push_back(DataSet::robot_pose.x());
            robot_pos.push_back(DataSet::robot_pose.y());
            robot_pos.push_back(DataSet::robot_pose.angle());
            // 获取地图数据
            if (ipa_room_plan->prepareData(robot_pos))
            {
                // 发布房屋分割指令
                if (ipa_room_plan->calRoomSegmentation())
                {
                    // 计算房屋排序
                    if (ipa_room_plan->calRoomSequence())
                    {
                        ipa_room_plan->roomID() = 0;
                        ipa_room_plan->maxRoom() = ipa_room_plan->result_seg->room_information_in_pixel.size();
                        while (ipa_room_plan->roomID() < ipa_room_plan->maxRoom() && rclcpp::ok())
                        {
                            std::cout << "----------- roomExploration ------------" << std::endl;
                            if (ipa_room_plan->roomExploration(ipa_room_plan->roomID()) > 0)
                                ipa_room_plan->roomID()++;
//                            else break;
                        }
                        finished = true;
                        return;
                    }
                }
            }
            finished = false;
            return;
        }
        else finished = true;
    }
    else finished = true;
}

void RoomDividerPlanner::SetNode(const rclcpp::Node::SharedPtr &ros_node,
                                 std::shared_ptr<IpaRoomPlanning> &ipa_room_plan_)
{
    ros_node_ = ros_node;
    ipa_room_plan = ipa_room_plan_;
}


} // namespace behavior_controller